#include <cstring>
#include <algorithm>
#include <gvars3/instances.h>
#include <gvars3/GStringUtil.h>
#include "PolygonalMesh.h"

using namespace GVars3;

PolygonalMesh::PolygonalMesh()
{
  static gvar3<double> gvdGridSize("PolygonalMesh.GridSize", 0.05, SILENT);
  mdGridSize = *gvdGridSize;
}


PolygonalMesh::~PolygonalMesh()
{
  Reset();
}


void PolygonalMesh::Reset()
{
  rtpCubes.RemoveAll();
}

void PolygonalMesh::RemoveCube(int ci, int cj, int ck)
{
  int index[3] = {ci,cj,ck};
  boost::shared_ptr<Cube> c;
  rtpCubes.Remove(index, c);
  c.reset();
}


void PolygonalMesh::RemoveCubes(int cimin, int cjmin, int ckmin, int cimax, int cjmax, int ckmax)
{
  int index_min[3] = {cimin,cjmin,ckmin};
  int index_max[3] = {cimax,cjmax,ckmax};
  std::vector<boost::shared_ptr<Cube> > cubes;

  rtpCubes.Remove(index_min, index_max, cubes);
  cubes.clear();
}

void PolygonalMesh::SetCube(int ci, int cj, int ck, boost::shared_ptr<Cube> c)
{
  int index[3] = {ci,cj,ck};
  boost::shared_ptr<Cube> oldc;

  rtpCubes.InsertOverwrite(index, c, oldc);  //Should never overwrite if using RemoveFaces first, but does not hurt to double check
  oldc.reset();
}

boost::shared_ptr<Cube> PolygonalMesh::GetCube(int ci, int cj, int ck)
{
  int index[3] = {ci,cj,ck};
  std::vector<boost::shared_ptr<Cube> > c;

  rtpCubes.Search(index, c);

  if(!c.empty())
    return c[0];
  else
    return boost::shared_ptr<Cube>();
}

int PolygonalMesh::GetCubes(int cimin, int cjmin, int ckmin, int cimax, int cjmax, int ckmax, std::vector<boost::shared_ptr<Cube> > &c)
{
  int index_min[3] = {cimin,cjmin,ckmin};
  int index_max[3] = {cimax,cjmax,ckmax};

  return rtpCubes.Search(index_min, index_max, c);
}

int PolygonalMesh::GetCubes(float xmin, float ymin, float zmin, float xmax, float ymax, float zmax, std::vector<boost::shared_ptr<Cube> > &c)
{
  int index_min[3] = {(int)floor(xmin/mdGridSize),(int)floor(ymin/mdGridSize),(int)floor(zmin/mdGridSize)};
  int index_max[3] = {(int)ceil(xmax/mdGridSize),(int)ceil(ymax/mdGridSize),(int)ceil(zmax/mdGridSize)};

  return rtpCubes.Search(index_min, index_max, c);
}

int PolygonalMesh::GetAllCubes(std::vector<boost::shared_ptr<Cube> > &c)
{
  return rtpCubes.GetAll(c);
}
